#include "algoritmos.h"
#include <iostream>

using std::cout;
using std::endl;

Algoritmos::Algoritmos()
{
    this->initialize();
}

void Algoritmos::initialize()
{
    //AL PASAR ALGORITMOS OPTIMIZAR CON HEXA
    offset = 0;
    index = 0;
    indexTemp = 0;
    countSamples = 0;
    for(int i=0; i < 20000; i++)
    {
        aceleracion[i] = 0;
        velocidad[i] = 0;
        posicion[i] = 0;
    }
    for(int i=0; i<8; i++)
    {
        tempAcel[i] = 0;
    }
    endMovement = true;
    startSpeed = false;
    reachZero = false;
}

void Algoritmos::integrar(int value)
{
    if(index == 20000) index = 0;
    value -= offset;
    double valor = value*(3.3/(1023*0.800));//206 sensible, menos sensible: 800
    valor = valor * 9.8;

    //MECHANICAL FILTERING WINDOW
    if(endMovement && (valor < 0.04 && valor > -0.04))
    {
        valor = 0;
    }
    else
    {
        endMovement = false;
        startSpeed = true;
    }

    aceleracion[index+1] = valor;

    if(!movementEndCheck(&valor))
    {
        double speed = velocidad[index] + (( aceleracion[index+1] + aceleracion[index] ) * TS / 2 );
        if(startSpeed && speed == 0)
        {
            reachZero = true;
        }

        if((startSpeed && reachZero) && (speed < 0.04 && speed > -0.04))
        {
            velocidad[index+1] = 0;
        }
        else
        {
            velocidad[index+1] = speed;
        }
        velocidad[index+1] = speed;
    }
    posicion[index+1] = posicion[index] + ((TS * ( velocidad[index+1] - velocidad[index] )) / 2) + velocidad[index]*TS;

    index++;
}

bool Algoritmos::movementEndCheck(double* valor)
{
    if(*valor == 0)
    {
        countSamples++;
        if(countSamples == 2) endMovement = true;
    }
    else
    {
        countSamples = 0;
    }

    if(countSamples >= 8)
    {
        velocidad[index+1] = 0;
        return true;
    }

    return false;
}

void Algoritmos::calibrar(double *val)
{
    for(int i=0; i < CALIBRACION; i++)
    {
        offset += val[i];
    }

    offset = offset / CALIBRACION;
}

void Algoritmos::filtrar(double *val)
{
    int count = 0;
    int value = 0;
    do{
        value += val[count];
        count++;
    }while(count != FILTRO);

    value = value / FILTRO;

    count = 0;
    do{
        val[count] = value - offset;
        count++;
    }while(count != FILTRO);
}

int Algoritmos::getOffset()
{
    return this->offset;
}

double Algoritmos::getPosicion()
{
    return posicion[index];
}

double* Algoritmos::vectorAceleracion()
{
    return &aceleracion[0];
}

double* Algoritmos::vectorVelocidad()
{
    return &velocidad[0];
}

double* Algoritmos::vectorPosicion()
{
    return &posicion[0];
}

